import airsim
import numpy as np

# 连接到AirSim模拟器
client = airsim.MultirotorClient()
client.confirmConnection()


def get_world_size():
    """获取并计算AirSim模拟世界的大小"""
    # 初始化 size 避免未定义错误
    size = None

    # 获取世界范围
    bounds = client.simGetWorldExtents()

    if not bounds:
        print("无法获取世界范围信息，请确保AirSim模拟器正在运行且地图已加载。")
        return size  # 或者根据需求返回默认值如 [0, 0, 0]

    # 检查返回类型是GeoPoint还是Vector3r
    if hasattr(bounds[0], 'latitude') and hasattr(bounds[0], 'longitude'):
        # 处理GeoPoint类型
        min_point = np.array([bounds[0].latitude, bounds[0].longitude, bounds[0].altitude])
        max_point = np.array([bounds[1].latitude, bounds[1].longitude, bounds[1].altitude])

        print(f"世界范围 (地理坐标):")
        print(f"  最小点: ({min_point[0]:.6f}, {min_point[1]:.6f}, {min_point[2]:.2f} m)")
        print(f"  最大点: ({max_point[0]:.6f}, {max_point[1]:.6f}, {max_point[2]:.2f} m)")

        # 计算近似距离（注意：这只是一个近似，因为地球是球体）
        avg_latitude = (min_point[0] + max_point[0]) / 2
        lat_to_m = 111320  # 1度纬度 ≈ 111,320米
        lon_to_m = 111320 * np.cos(np.radians(avg_latitude))  # 1度经度在给定纬度处的近似米数

        size = np.array([
            abs(max_point[0] - min_point[0]) * lat_to_m,
            abs(max_point[1] - min_point[1]) * lon_to_m,
            abs(max_point[2] - min_point[2])  # 高度已经是米
        ])

        print(f"  近似大小: ({size[0]:.2f}, {size[1]:.2f}, {size[2]:.2f}) 米")

    else:
        # 处理Vector3r类型（假设单位为厘米，转换为米）
        min_point = np.array([bounds[0].x_val / 100, bounds[0].y_val / 100, bounds[0].z_val / 100])
        max_point = np.array([bounds[1].x_val / 100, bounds[1].y_val / 100, bounds[1].z_val / 100])

        print(f"世界范围 (笛卡尔坐标):")
        print(f"  最小点: ({min_point[0]:.2f}, {min_point[1]:.2f}, {min_point[2]:.2f}) 米")
        print(f"  最大点: ({max_point[0]:.2f}, {max_point[1]:.2f}, {max_point[2]:.2f}) 米")

        size = max_point - min_point
        print(f"  大小: ({size[0]:.2f}, {size[1]:.2f}, {size[2]:.2f}) 米")

    return size


if __name__ == "__main__":
    world_size = get_world_size()
    if world_size is not None:
        print("成功获取世界尺寸:", world_size)
    else:
        print("未能获取世界尺寸。")
